/*
 * File: TungSim.c
 *
 * Real-Time Workshop code generated for Simulink model TungSim.
 *
 * Model version                        : 1.66
 * Real-Time Workshop file version      : 7.6  (R2010b)  03-Aug-2010
 * Real-Time Workshop file generated on : Wed Sep 12 17:02:05 2012
 * TLC version                          : 7.6 (Jul 13 2010)
 * C/C++ source code generated on       : Wed Sep 12 17:02:06 2012
 *
 * Target selection: ert.tlc
 * Embedded hardware selection: ARM Compatible->ARM 7
 * Code generation objectives: Unspecified
 * Validation result: Not run
 */

#include "TungSim.h"
#include "TungSim_private.h"
#include "stm32f4xx_gpio.h"

/* Exported block signals */
real_T fgyro[3];                       /* '<Root>/Gyro  Sensor' */
real_T facc[3];                        /* '<Root>/Acc  Sensor' */
real_T fmag[3];                        /* '<Root>/Mag  Sensor' */
real_T Out1[3];                        /* '<Root>/Gain2' */
real_T Out2;                           /* '<Root>/Pulse Generator' */

/* Block states (auto storage) */
D_Work_TungSim TungSim_DWork;

/* Real-time model */
RT_MODEL_TungSim TungSim_M_;
RT_MODEL_TungSim *TungSim_M = &TungSim_M_;

/* Forward declaration for local functions */
static void TungSim_inv(const real_T x[36], real_T y[36]);
static real_T TungSim_norm(const real_T x[2]);
static void TungSim_inv_i(const real_T x[36], real_T y[36]);

/* Function for Embedded MATLAB: '<S4>/Embedded MATLAB Function' */
static void TungSim_inv(const real_T x[36], real_T y[36])
{
  int8_T p[6];
  int32_T c;
  real_T A[36];
  int8_T ipiv[6];
  int32_T mmj;
  int32_T jj;
  int32_T jp1j;
  int32_T jpiv_offset;
  int32_T jrow;
  int32_T ix;
  real_T smax;
  real_T s;
  int32_T jA;
  int32_T pipk;
  int32_T i;
  for (i = 0; i < 36; i++) {
    y[i] = 0.0;
    A[i] = x[i];
  }

  for (i = 0; i < 6; i++) {
    ipiv[i] = (int8_T)(1 + i);
  }

  for (i = 0; i < 5; i++) {
    mmj = 5 - i;
    jj = i * 7 + 1;
    jp1j = jj + 1;
    pipk = mmj + 1;
    c = 1;
    ix = jj;
    smax = fabs(A[jj - 1]);
    for (jrow = 2; jrow <= pipk; jrow++) {
      ix++;
      s = fabs(A[ix - 1]);
      if (s > smax) {
        c = jrow;
        smax = s;
      }
    }

    jpiv_offset = c - 1;
    if (A[(jj + jpiv_offset) - 1] != 0.0) {
      if (jpiv_offset != 0) {
        ipiv[i] = (int8_T)((i + 1) + jpiv_offset);
        jrow = 1 + i;
        pipk = jrow + jpiv_offset;
        for (c = 0; c < 6; c++) {
          smax = A[jrow - 1];
          A[jrow - 1] = A[pipk - 1];
          A[pipk - 1] = smax;
          jrow += 6;
          pipk += 6;
        }
      }

      pipk = (mmj - 1) + jp1j;
      for (c = jp1j; c <= pipk; c++) {
        A[c - 1] = A[c - 1] / A[jj - 1];
      }
    }

    jrow = 5 - i;
    jA = jj + 6;
    pipk = jj + 6;
    for (c = 1; c <= jrow; c++) {
      if (A[pipk - 1] != 0.0) {
        smax = A[pipk - 1] * -1.0;
        jj = jp1j;
        jpiv_offset = mmj + jA;
        for (ix = 1 + jA; ix <= jpiv_offset; ix++) {
          A[ix - 1] = A[jj - 1] * smax + A[ix - 1];
          jj++;
        }
      }

      pipk += 6;
      jA += 6;
    }
  }

  for (i = 0; i < 6; i++) {
    p[i] = (int8_T)(1 + i);
  }

  for (jpiv_offset = 0; jpiv_offset < 5; jpiv_offset++) {
    c = ipiv[jpiv_offset];
    if (c > jpiv_offset + 1) {
      pipk = p[c - 1];
      p[c - 1] = p[jpiv_offset];
      p[jpiv_offset] = (int8_T)pipk;
    }
  }

  for (jpiv_offset = 0; jpiv_offset < 6; jpiv_offset++) {
    c = p[jpiv_offset];
    y[jpiv_offset + 6 * (c - 1)] = 1.0;
    for (pipk = jpiv_offset + 1; pipk < 7; pipk++) {
      if (y[(c - 1) * 6 + (pipk - 1)] != 0.0) {
        for (ix = pipk + 1; ix < 7; ix++) {
          y[(ix - 1) + 6 * (c - 1)] = y[(c - 1) * 6 + (ix - 1)] - y[(c - 1) * 6
            + (pipk - 1)] * A[(pipk - 1) * 6 + (ix - 1)];
        }
      }
    }
  }

  for (jpiv_offset = 0; jpiv_offset < 6; jpiv_offset++) {
    pipk = 6 * jpiv_offset;
    for (c = 6; c > 0; c += -1) {
      ix = (c - 1) * 6;
      if (y[(c + pipk) - 1] != 0.0) {
        y[(c + pipk) - 1] = y[(c + pipk) - 1] / A[(c + ix) - 1];
        jrow = c - 1;
        for (jj = 1; jj <= jrow; jj++) {
          y[(jj + pipk) - 1] = y[(jj + pipk) - 1] - y[(c + pipk) - 1] * A[(jj +
            ix) - 1];
        }
      }
    }
  }
}

/* Function for Embedded MATLAB: '<S1>/Compute z2' */
static real_T TungSim_norm(const real_T x[2])
{
  real_T y;
  real_T scale;
  boolean_T firstNonZero;
  real_T absxk;
  real_T t;
  y = 0.0;
  scale = 0.0;
  firstNonZero = TRUE;
  if (x[0] != 0.0) {
    scale = fabs(x[0]);
    y = 1.0;
    firstNonZero = FALSE;
  }

  if (x[1] != 0.0) {
    absxk = fabs(x[1]);
    if (firstNonZero) {
      scale = absxk;
      y = 1.0;
    } else if (scale < absxk) {
      t = scale / absxk;
      y = y * t * t + 1.0;
      scale = absxk;
    } else {
      t = absxk / scale;
      y += t * t;
    }
  }

  return scale * sqrt(y);
}

/* Function for Embedded MATLAB: '<S5>/Embedded MATLAB Function' */
static void TungSim_inv_i(const real_T x[36], real_T y[36])
{
  int8_T p[6];
  int32_T c;
  real_T A[36];
  int8_T ipiv[6];
  int32_T mmj;
  int32_T jj;
  int32_T jp1j;
  int32_T jpiv_offset;
  int32_T jrow;
  int32_T ix;
  real_T smax;
  real_T s;
  int32_T jA;
  int32_T pipk;
  int32_T i;
  for (i = 0; i < 36; i++) {
    y[i] = 0.0;
    A[i] = x[i];
  }

  for (i = 0; i < 6; i++) {
    ipiv[i] = (int8_T)(1 + i);
  }

  for (i = 0; i < 5; i++) {
    mmj = 5 - i;
    jj = i * 7 + 1;
    jp1j = jj + 1;
    pipk = mmj + 1;
    c = 1;
    ix = jj;
    smax = fabs(A[jj - 1]);
    for (jrow = 2; jrow <= pipk; jrow++) {
      ix++;
      s = fabs(A[ix - 1]);
      if (s > smax) {
        c = jrow;
        smax = s;
      }
    }

    jpiv_offset = c - 1;
    if (A[(jj + jpiv_offset) - 1] != 0.0) {
      if (jpiv_offset != 0) {
        ipiv[i] = (int8_T)((i + 1) + jpiv_offset);
        jrow = 1 + i;
        pipk = jrow + jpiv_offset;
        for (c = 0; c < 6; c++) {
          smax = A[jrow - 1];
          A[jrow - 1] = A[pipk - 1];
          A[pipk - 1] = smax;
          jrow += 6;
          pipk += 6;
        }
      }

      pipk = (mmj - 1) + jp1j;
      for (c = jp1j; c <= pipk; c++) {
        A[c - 1] = A[c - 1] / A[jj - 1];
      }
    }

    jrow = 5 - i;
    jA = jj + 6;
    pipk = jj + 6;
    for (c = 1; c <= jrow; c++) {
      if (A[pipk - 1] != 0.0) {
        smax = A[pipk - 1] * -1.0;
        jj = jp1j;
        jpiv_offset = mmj + jA;
        for (ix = 1 + jA; ix <= jpiv_offset; ix++) {
          A[ix - 1] = A[jj - 1] * smax + A[ix - 1];
          jj++;
        }
      }

      pipk += 6;
      jA += 6;
    }
  }

  for (i = 0; i < 6; i++) {
    p[i] = (int8_T)(1 + i);
  }

  for (jpiv_offset = 0; jpiv_offset < 5; jpiv_offset++) {
    c = ipiv[jpiv_offset];
    if (c > jpiv_offset + 1) {
      pipk = p[c - 1];
      p[c - 1] = p[jpiv_offset];
      p[jpiv_offset] = (int8_T)pipk;
    }
  }

  for (jpiv_offset = 0; jpiv_offset < 6; jpiv_offset++) {
    c = p[jpiv_offset];
    y[jpiv_offset + 6 * (c - 1)] = 1.0;
    for (pipk = jpiv_offset + 1; pipk < 7; pipk++) {
      if (y[(c - 1) * 6 + (pipk - 1)] != 0.0) {
        for (ix = pipk + 1; ix < 7; ix++) {
          y[(ix - 1) + 6 * (c - 1)] = y[(c - 1) * 6 + (ix - 1)] - y[(c - 1) * 6
            + (pipk - 1)] * A[(pipk - 1) * 6 + (ix - 1)];
        }
      }
    }
  }

  for (jpiv_offset = 0; jpiv_offset < 6; jpiv_offset++) {
    pipk = 6 * jpiv_offset;
    for (c = 6; c > 0; c += -1) {
      ix = (c - 1) * 6;
      if (y[(c + pipk) - 1] != 0.0) {
        y[(c + pipk) - 1] = y[(c + pipk) - 1] / A[(c + ix) - 1];
        jrow = c - 1;
        for (jj = 1; jj <= jrow; jj++) {
          y[(jj + pipk) - 1] = y[(jj + pipk) - 1] - y[(c + pipk) - 1] * A[(jj +
            ix) - 1];
        }
      }
    }
  }
}

real_T rt_atan2d_snf(real_T u0, real_T u1)
{
  if (rtIsNaN(u0) || rtIsNaN(u1)) {
    return (rtNaN);
  } else if (rtIsInf(u0) && rtIsInf(u1)) {
    return atan2(u0 > 0.0 ? 1.0 : -1.0, u1 > 0.0 ? 1.0 : -1.0);
  } else if (u1 == 0.0) {
    if (u0 > 0.0) {
      return RT_PI / 2.0;
    } else if (u0 < 0.0) {
      return -(RT_PI / 2.0);
    } else {
      return 0.0;
    }
  } else {
    return atan2(u0, u1);
  }
}

/* Model step function */
void TungSim_step(void)
{
  real_T C[9];
  real_T Phik[36];
  real_T x[6];
  real_T P[36];
  int8_T I[9];
  int8_T b_I[9];
  int8_T c_I[36];
  real_T scale;
  boolean_T firstNonZero;
  real_T theta;
  real_T cos_theta;
  real_T sin_phi;
  real_T cos_phi;
  real_T Xh;
  real_T Yh;
  real_T cos_psi;
  real_T rtb_x_e[6];
  real_T rtb_x[6];
  real_T rtb_P_j[36];
  real_T rtb_P[36];
  real_T tmp[36];
  real_T Xh_0[2];
  real_T tmp_0[36];
  int32_T i;
  real_T Phik_0[36];
  real_T tmp_1[9];
  real_T Phik_1[36];
  real_T tmp_2[36];
  real_T rtb_Gain[6];
  int32_T i_0;
  int32_T i_1;
  real_T rtb_Gain1_idx;
  real_T rtb_Gain1_idx_0;
  static real_T tmp_3[9] = { 0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01 };

  static int8_T tmp_4[36] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0,
    0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1 };

  static real_T tmp_5[36] = { 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
    0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
    0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 };

  static real_T tmp_6[36] = { 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0,
    0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0,
    0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5 };

  {
    /* user code (Output function Header) */

    /* System '<Root>' */
    uint16_t i;

    /* user code (Output function Body) */

    /* System '<Root>' */

    /* Gain: '<Root>/Gain1' incorporates:
     *  Inport: '<Root>/Mag  Sensor'
     */
    rtb_Gain1_idx_0 = TungSim_P.Gain1_Gain * fmag[1];
    rtb_Gain1_idx = TungSim_P.Gain1_Gain * fmag[2];

    /* Embedded MATLAB: '<S4>/Embedded MATLAB Function' incorporates:
     *  Inport: '<Root>/Acc  Sensor'
     *  Inport: '<Root>/Gyro  Sensor'
     *  SignalConversion: '<S6>/TmpSignal ConversionAt SFunction Inport3'
     *  UnitDelay: '<S1>/Unit Delay'
     *  UnitDelay: '<S1>/Unit Delay1'
     */
    /* Embedded MATLAB Function 'DCM Filter/KF1 estimates phi & theta/Embedded MATLAB Function': '<S6>:1' */
    /*  x_1: (6x1) previous estimate value [C31;C32;C33;gx;gy;gz] */
    /*  P_1: (6x6) previous error covariance */
    /*  z  : (6x1) observer value [ax;ay;az;gx;gy;gz] */
    /*  x  : (6x1) estimation value at this time  */
    /*  P  : (6x6) error covariance */
    /* % Init Kalman */
    /* 0.4 */
    /*  ra0 = r; */
    /*  rg0 = 1/a; */
    /* '<S6>:1:17' */
    /*  Note: a is divided by 'g' --> H = I; */
    /* % Discrete model */
    /* '<S6>:1:22' */
    /* '<S6>:1:23' */
    /* '<S6>:1:24' */
    /* '<S6>:1:25' */
    C[0] = 0.0;
    C[3] = -TungSim_DWork.UnitDelay_DSTATE[2];
    C[6] = TungSim_DWork.UnitDelay_DSTATE[1];
    C[1] = TungSim_DWork.UnitDelay_DSTATE[2];
    C[4] = 0.0;
    C[7] = -TungSim_DWork.UnitDelay_DSTATE[0];
    C[2] = -TungSim_DWork.UnitDelay_DSTATE[1];
    C[5] = TungSim_DWork.UnitDelay_DSTATE[0];
    C[8] = 0.0;

    /* '<S6>:1:26' */
    for (i = 0; i < 9; i++) {
      I[i] = 0;
      b_I[i] = 0;
    }

    I[0] = 1;
    I[4] = 1;
    I[8] = 1;
    b_I[0] = 1;
    b_I[4] = 1;
    b_I[8] = 1;
    for (i = 0; i < 3; i++) {
      Phik[6 * i] = (real_T)I[3 * i];
      Phik[1 + 6 * i] = (real_T)I[3 * i + 1];
      Phik[2 + 6 * i] = (real_T)I[3 * i + 2];
    }

    for (i = 0; i < 3; i++) {
      Phik[6 * (i + 3)] = C[3 * i] * 0.01;
      Phik[1 + 6 * (i + 3)] = C[3 * i + 1] * 0.01;
      Phik[2 + 6 * (i + 3)] = C[3 * i + 2] * 0.01;
    }

    for (i = 0; i < 3; i++) {
      Phik[3 + 6 * i] = 0.0;
      Phik[4 + 6 * i] = 0.0;
      Phik[5 + 6 * i] = 0.0;
    }

    for (i = 0; i < 3; i++) {
      Phik[3 + 6 * (i + 3)] = (real_T)b_I[3 * i];
      Phik[4 + 6 * (i + 3)] = (real_T)b_I[3 * i + 1];
      Phik[5 + 6 * (i + 3)] = (real_T)b_I[3 * i + 2];
    }

    /*  Phik = I + Phi(kT) * T */
    /* '<S6>:1:27' */
    /* % Time update */
    /*  State */
    /* '<S6>:1:31' */
    for (i = 0; i < 6; i++) {
      x[i] = 0.0;
      for (i_0 = 0; i_0 < 6; i_0++) {
        x[i] = Phik[6 * i_0 + i] * TungSim_DWork.UnitDelay_DSTATE[i_0] + x[i];
      }
    }

    /*  Error covariance */
    /* '<S6>:1:33' */
    for (i = 0; i < 6; i++) {
      for (i_0 = 0; i_0 < 6; i_0++) {
        Phik_0[i + 6 * i_0] = 0.0;
        for (i_1 = 0; i_1 < 6; i_1++) {
          Phik_0[i + 6 * i_0] = Phik[6 * i_1 + i] *
            TungSim_DWork.UnitDelay1_DSTATE[6 * i_0 + i_1] + Phik_0[6 * i_0 + i];
        }
      }
    }

    for (i = 0; i < 3; i++) {
      for (i_0 = 0; i_0 < 3; i_0++) {
        tmp_1[i + 3 * i_0] = 0.0;
        tmp_1[i + 3 * i_0] = tmp_1[3 * i_0 + i] + 1.0000000000000002E-6 * C[i] *
          C[i_0];
        tmp_1[i + 3 * i_0] = C[i + 3] * 1.0000000000000002E-6 * C[i_0 + 3] +
          tmp_1[3 * i_0 + i];
        tmp_1[i + 3 * i_0] = C[i + 6] * 1.0000000000000002E-6 * C[i_0 + 6] +
          tmp_1[3 * i_0 + i];
      }
    }

    for (i = 0; i < 6; i++) {
      for (i_0 = 0; i_0 < 6; i_0++) {
        Phik_1[i + 6 * i_0] = 0.0;
        for (i_1 = 0; i_1 < 6; i_1++) {
          Phik_1[i + 6 * i_0] = Phik_0[6 * i_1 + i] * Phik[6 * i_1 + i_0] +
            Phik_1[6 * i_0 + i];
        }
      }
    }

    for (i = 0; i < 3; i++) {
      tmp_2[6 * i] = tmp_1[3 * i];
      tmp_2[1 + 6 * i] = tmp_1[3 * i + 1];
      tmp_2[2 + 6 * i] = tmp_1[3 * i + 2];
    }

    for (i = 0; i < 3; i++) {
      tmp_2[6 * (i + 3)] = C[3 * i] * 0.0001;
      tmp_2[1 + 6 * (i + 3)] = C[3 * i + 1] * 0.0001;
      tmp_2[2 + 6 * (i + 3)] = C[3 * i + 2] * 0.0001;
    }

    for (i = 0; i < 3; i++) {
      tmp_2[3 + 6 * i] = 0.0001 * C[i];
      tmp_2[4 + 6 * i] = C[i + 3] * 0.0001;
      tmp_2[5 + 6 * i] = C[i + 6] * 0.0001;
    }

    for (i = 0; i < 3; i++) {
      tmp_2[3 + 6 * (i + 3)] = tmp_3[3 * i];
      tmp_2[4 + 6 * (i + 3)] = tmp_3[3 * i + 1];
      tmp_2[5 + 6 * (i + 3)] = tmp_3[3 * i + 2];
    }

    for (i = 0; i < 6; i++) {
      for (i_0 = 0; i_0 < 6; i_0++) {
        P[i_0 + 6 * i] = Phik_1[6 * i + i_0] + tmp_2[6 * i + i_0];
      }
    }

    /* % Measurement update */
    /*  Compute Kalman gain */
    /* '<S6>:1:37' */
    for (i = 0; i < 6; i++) {
      for (i_0 = 0; i_0 < 6; i_0++) {
        tmp_2[i + 6 * i_0] = 0.0;
        for (i_1 = 0; i_1 < 6; i_1++) {
          tmp_2[i + 6 * i_0] = (real_T)tmp_4[6 * i_1 + i] * P[6 * i_0 + i_1] +
            tmp_2[6 * i_0 + i];
        }
      }
    }

    for (i = 0; i < 6; i++) {
      for (i_0 = 0; i_0 < 6; i_0++) {
        Yh = 0.0;
        for (i_1 = 0; i_1 < 6; i_1++) {
          Yh += tmp_2[6 * i_1 + i] * (real_T)tmp_4[6 * i_0 + i_1];
        }

        tmp_0[i + 6 * i_0] = tmp_6[6 * i_0 + i] + Yh;
      }
    }

    TungSim_inv(tmp_0, tmp_2);
    for (i = 0; i < 6; i++) {
      for (i_0 = 0; i_0 < 6; i_0++) {
        Phik_0[i + 6 * i_0] = 0.0;
        for (i_1 = 0; i_1 < 6; i_1++) {
          Phik_0[i + 6 * i_0] = P[6 * i_1 + i] * (real_T)tmp_4[6 * i_0 + i_1] +
            Phik_0[6 * i_0 + i];
        }
      }
    }

    for (i = 0; i < 6; i++) {
      for (i_0 = 0; i_0 < 6; i_0++) {
        Phik[i + 6 * i_0] = 0.0;
        for (i_1 = 0; i_1 < 6; i_1++) {
          Phik[i + 6 * i_0] = Phik_0[6 * i_1 + i] * tmp_2[6 * i_0 + i_1] + Phik
            [6 * i_0 + i];
        }
      }
    }

    /*  Update estimate with measurement z */
    /* '<S6>:1:39' */
    rtb_x[0] = TungSim_P.Gain1_Gain * facc[0];
    rtb_x[1] = TungSim_P.Gain1_Gain * facc[1];
    rtb_x[2] = TungSim_P.Gain1_Gain * facc[2];
    rtb_x[3] = TungSim_P.Gain1_Gain * fgyro[0];
    rtb_x[4] = TungSim_P.Gain1_Gain * fgyro[1];
    rtb_x[5] = TungSim_P.Gain1_Gain * fgyro[2];
    for (i = 0; i < 6; i++) {
      Yh = 0.0;
      for (i_0 = 0; i_0 < 6; i_0++) {
        Yh += (real_T)tmp_4[6 * i_0 + i] * x[i_0];
      }

      rtb_Gain[i] = rtb_x[i] - Yh;
    }

    for (i = 0; i < 6; i++) {
      Yh = 0.0;
      for (i_0 = 0; i_0 < 6; i_0++) {
        Yh += Phik[6 * i_0 + i] * rtb_Gain[i_0];
      }

      rtb_x_e[i] = x[i] + Yh;
    }

    /*  Normalization DCM(1:3) */
    /* '<S6>:1:41' */
    Yh = 0.0;
    scale = 0.0;
    firstNonZero = TRUE;
    if (rtb_x_e[0] != 0.0) {
      scale = fabs(rtb_x_e[0]);
      Yh = 1.0;
      firstNonZero = FALSE;
    }

    if (rtb_x_e[1] != 0.0) {
      cos_psi = fabs(rtb_x_e[1]);
      if (firstNonZero) {
        scale = cos_psi;
        Yh = 1.0;
        firstNonZero = FALSE;
      } else if (scale < cos_psi) {
        Xh = scale / cos_psi;
        Yh = Yh * Xh * Xh + 1.0;
        scale = cos_psi;
      } else {
        Xh = cos_psi / scale;
        Yh += Xh * Xh;
      }
    }

    if (rtb_x_e[2] != 0.0) {
      cos_psi = fabs(rtb_x_e[2]);
      if (firstNonZero) {
        scale = cos_psi;
        Yh = 1.0;
      } else if (scale < cos_psi) {
        Xh = scale / cos_psi;
        Yh = Yh * Xh * Xh + 1.0;
        scale = cos_psi;
      } else {
        Xh = cos_psi / scale;
        Yh += Xh * Xh;
      }
    }

    Yh = scale * sqrt(Yh);
    rtb_x_e[0] = rtb_x_e[0] / Yh;
    rtb_x_e[1] = rtb_x_e[1] / Yh;
    rtb_x_e[2] = rtb_x_e[2] / Yh;

    /*  Update the error covariance */
    /* '<S6>:1:43' */
    for (i = 0; i < 36; i++) {
      c_I[i] = 0;
    }

    for (i = 0; i < 6; i++) {
      c_I[i + 6 * i] = 1;
    }

    for (i = 0; i < 6; i++) {
      for (i_0 = 0; i_0 < 6; i_0++) {
        Yh = 0.0;
        for (i_1 = 0; i_1 < 6; i_1++) {
          Yh += Phik[6 * i_1 + i] * (real_T)tmp_4[6 * i_0 + i_1];
        }

        Phik_0[i + 6 * i_0] = (real_T)c_I[6 * i_0 + i] - Yh;
      }
    }

    for (i = 0; i < 6; i++) {
      for (i_0 = 0; i_0 < 6; i_0++) {
        rtb_P_j[i + 6 * i_0] = 0.0;
        for (i_1 = 0; i_1 < 6; i_1++) {
          rtb_P_j[i + 6 * i_0] = Phik_0[6 * i_1 + i] * P[6 * i_0 + i_1] +
            rtb_P_j[6 * i_0 + i];
        }
      }
    }

    /* Embedded MATLAB: '<S1>/Compute z2' incorporates:
     *  Inport: '<Root>/Mag  Sensor'
     */
    /* Embedded MATLAB Function 'DCM Filter/Compute z2': '<S3>:1' */
    /*  x_kf1 = [C31;C32;C33;gx;gy;gz]; */
    /*  m = [mx;my;mz]; */
    /*  Get measurement data: Magnetometer, phi & theta from above result */
    /* '<S3>:1:6' */
    scale = -rtb_x_e[0];

    /* '<S3>:1:7' */
    theta = asin(scale);

    /* '<S3>:1:8' */
    cos_theta = cos(theta);

    /* '<S3>:1:10' */
    /* '<S3>:1:11' */
    sin_phi = rtb_x_e[1] / cos_theta;

    /*  reduce computation */
    /* '<S3>:1:12' */
    cos_phi = rtb_x_e[2] / cos_theta;

    /* '<S3>:1:14' */
    /* '<S3>:1:15' */
    Xh = (TungSim_P.Gain1_Gain * fmag[0] * cos_theta + rtb_Gain1_idx_0 * scale *
          sin_phi) + rtb_Gain1_idx * scale * cos_phi;

    /* '<S3>:1:16' */
    Yh = rtb_Gain1_idx_0 * cos_phi - rtb_Gain1_idx * sin_phi;

    /* '<S3>:1:17' */
    Xh_0[0] = Xh;
    Xh_0[1] = Yh;
    rtb_Gain1_idx_0 = (-Yh) / TungSim_norm(Xh_0);

    /* '<S3>:1:18' */
    Xh_0[0] = Xh;
    Xh_0[1] = Yh;
    cos_psi = Xh / TungSim_norm(Xh_0);

    /* '<S3>:1:20' */

    /* Embedded MATLAB: '<S5>/Embedded MATLAB Function' incorporates:
     *  UnitDelay: '<S1>/Unit Delay2'
     *  UnitDelay: '<S1>/Unit Delay3'
     */
    /* Embedded MATLAB Function 'DCM Filter/KF2 estimates psi/Embedded MATLAB Function': '<S7>:1' */
    /*  x_1: (6x1) previous estimate value [C21;C22;C23;gx;gy;gz] */
    /*  P_1: (6x6) previous error covariance */
    /*  z  : (6x1) observer value [mx;my;mz;gx;gy;gz] */
    /*  x  : (6x1) estimation value at this time */
    /*  P  : (6x6) error covariance */
    /* % Init Kalman */
    /* '<S7>:1:14' */
    /* % Discrete model */
    /* '<S7>:1:19' */
    /* '<S7>:1:20' */
    /* '<S7>:1:21' */
    /* '<S7>:1:22' */
    C[0] = 0.0;
    C[3] = -TungSim_DWork.UnitDelay2_DSTATE[2];
    C[6] = TungSim_DWork.UnitDelay2_DSTATE[1];
    C[1] = TungSim_DWork.UnitDelay2_DSTATE[2];
    C[4] = 0.0;
    C[7] = -TungSim_DWork.UnitDelay2_DSTATE[0];
    C[2] = -TungSim_DWork.UnitDelay2_DSTATE[1];
    C[5] = TungSim_DWork.UnitDelay2_DSTATE[0];
    C[8] = 0.0;

    /* '<S7>:1:23' */
    for (i = 0; i < 9; i++) {
      I[i] = 0;
      b_I[i] = 0;
    }

    I[0] = 1;
    I[4] = 1;
    I[8] = 1;
    b_I[0] = 1;
    b_I[4] = 1;
    b_I[8] = 1;
    for (i = 0; i < 3; i++) {
      Phik[6 * i] = (real_T)I[3 * i];
      Phik[1 + 6 * i] = (real_T)I[3 * i + 1];
      Phik[2 + 6 * i] = (real_T)I[3 * i + 2];
    }

    for (i = 0; i < 3; i++) {
      Phik[6 * (i + 3)] = C[3 * i] * 0.01;
      Phik[1 + 6 * (i + 3)] = C[3 * i + 1] * 0.01;
      Phik[2 + 6 * (i + 3)] = C[3 * i + 2] * 0.01;
    }

    for (i = 0; i < 3; i++) {
      Phik[3 + 6 * i] = 0.0;
      Phik[4 + 6 * i] = 0.0;
      Phik[5 + 6 * i] = 0.0;
    }

    for (i = 0; i < 3; i++) {
      Phik[3 + 6 * (i + 3)] = (real_T)b_I[3 * i];
      Phik[4 + 6 * (i + 3)] = (real_T)b_I[3 * i + 1];
      Phik[5 + 6 * (i + 3)] = (real_T)b_I[3 * i + 2];
    }

    /*  Phik = I + Phi(kT) * T */
    /* '<S7>:1:24' */
    /* % Time update */
    /*  State */
    /* '<S7>:1:28' */
    for (i = 0; i < 6; i++) {
      x[i] = 0.0;
      for (i_0 = 0; i_0 < 6; i_0++) {
        x[i] = Phik[6 * i_0 + i] * TungSim_DWork.UnitDelay2_DSTATE[i_0] + x[i];
      }
    }

    /*  Error covariance */
    /* '<S7>:1:30' */
    for (i = 0; i < 6; i++) {
      for (i_0 = 0; i_0 < 6; i_0++) {
        Phik_0[i + 6 * i_0] = 0.0;
        for (i_1 = 0; i_1 < 6; i_1++) {
          Phik_0[i + 6 * i_0] = Phik[6 * i_1 + i] *
            TungSim_DWork.UnitDelay3_DSTATE[6 * i_0 + i_1] + Phik_0[6 * i_0 + i];
        }
      }
    }

    for (i = 0; i < 3; i++) {
      for (i_0 = 0; i_0 < 3; i_0++) {
        tmp_1[i + 3 * i_0] = 0.0;
        tmp_1[i + 3 * i_0] = tmp_1[3 * i_0 + i] + 1.0000000000000002E-6 * C[i] *
          C[i_0];
        tmp_1[i + 3 * i_0] = C[i + 3] * 1.0000000000000002E-6 * C[i_0 + 3] +
          tmp_1[3 * i_0 + i];
        tmp_1[i + 3 * i_0] = C[i + 6] * 1.0000000000000002E-6 * C[i_0 + 6] +
          tmp_1[3 * i_0 + i];
      }
    }

    for (i = 0; i < 6; i++) {
      for (i_0 = 0; i_0 < 6; i_0++) {
        Phik_1[i + 6 * i_0] = 0.0;
        for (i_1 = 0; i_1 < 6; i_1++) {
          Phik_1[i + 6 * i_0] = Phik_0[6 * i_1 + i] * Phik[6 * i_1 + i_0] +
            Phik_1[6 * i_0 + i];
        }
      }
    }

    for (i = 0; i < 3; i++) {
      tmp_2[6 * i] = tmp_1[3 * i];
      tmp_2[1 + 6 * i] = tmp_1[3 * i + 1];
      tmp_2[2 + 6 * i] = tmp_1[3 * i + 2];
    }

    for (i = 0; i < 3; i++) {
      tmp_2[6 * (i + 3)] = C[3 * i] * 0.0001;
      tmp_2[1 + 6 * (i + 3)] = C[3 * i + 1] * 0.0001;
      tmp_2[2 + 6 * (i + 3)] = C[3 * i + 2] * 0.0001;
    }

    for (i = 0; i < 3; i++) {
      tmp_2[3 + 6 * i] = 0.0001 * C[i];
      tmp_2[4 + 6 * i] = C[i + 3] * 0.0001;
      tmp_2[5 + 6 * i] = C[i + 6] * 0.0001;
    }

    for (i = 0; i < 3; i++) {
      tmp_2[3 + 6 * (i + 3)] = tmp_3[3 * i];
      tmp_2[4 + 6 * (i + 3)] = tmp_3[3 * i + 1];
      tmp_2[5 + 6 * (i + 3)] = tmp_3[3 * i + 2];
    }

    for (i = 0; i < 6; i++) {
      for (i_0 = 0; i_0 < 6; i_0++) {
        P[i_0 + 6 * i] = Phik_1[6 * i + i_0] + tmp_2[6 * i + i_0];
      }
    }

    /* % Measurement update */
    /*  Compute Kalman gain */
    /* '<S7>:1:34' */
    for (i = 0; i < 6; i++) {
      for (i_0 = 0; i_0 < 6; i_0++) {
        tmp_2[i + 6 * i_0] = 0.0;
        for (i_1 = 0; i_1 < 6; i_1++) {
          tmp_2[i + 6 * i_0] = (real_T)tmp_4[6 * i_1 + i] * P[6 * i_0 + i_1] +
            tmp_2[6 * i_0 + i];
        }
      }
    }

    for (i = 0; i < 6; i++) {
      for (i_0 = 0; i_0 < 6; i_0++) {
        Yh = 0.0;
        for (i_1 = 0; i_1 < 6; i_1++) {
          Yh += tmp_2[6 * i_1 + i] * (real_T)tmp_4[6 * i_0 + i_1];
        }

        tmp[i + 6 * i_0] = tmp_5[6 * i_0 + i] + Yh;
      }
    }

    TungSim_inv_i(tmp, tmp_2);
    for (i = 0; i < 6; i++) {
      for (i_0 = 0; i_0 < 6; i_0++) {
        Phik_0[i + 6 * i_0] = 0.0;
        for (i_1 = 0; i_1 < 6; i_1++) {
          Phik_0[i + 6 * i_0] = P[6 * i_1 + i] * (real_T)tmp_4[6 * i_0 + i_1] +
            Phik_0[6 * i_0 + i];
        }
      }
    }

    for (i = 0; i < 6; i++) {
      for (i_0 = 0; i_0 < 6; i_0++) {
        Phik[i + 6 * i_0] = 0.0;
        for (i_1 = 0; i_1 < 6; i_1++) {
          Phik[i + 6 * i_0] = Phik_0[6 * i_1 + i] * tmp_2[6 * i_0 + i_1] + Phik
            [6 * i_0 + i];
        }
      }
    }

    /*  Update estimate with measurement z */
    /* '<S7>:1:36' */
    rtb_x[0] = cos_theta * rtb_Gain1_idx_0;
    rtb_x[1] = sin_phi * scale * rtb_Gain1_idx_0 + cos_phi * cos_psi;
    rtb_x[2] = cos_phi * scale * rtb_Gain1_idx_0 + (-sin_phi) * cos_psi;
    rtb_x[3] = rtb_x_e[3];
    rtb_x[4] = rtb_x_e[4];
    rtb_x[5] = rtb_x_e[5];
    for (i = 0; i < 6; i++) {
      Yh = 0.0;
      for (i_0 = 0; i_0 < 6; i_0++) {
        Yh += (real_T)tmp_4[6 * i_0 + i] * x[i_0];
      }

      rtb_Gain[i] = rtb_x[i] - Yh;
    }

    for (i = 0; i < 6; i++) {
      Yh = 0.0;
      for (i_0 = 0; i_0 < 6; i_0++) {
        Yh += Phik[6 * i_0 + i] * rtb_Gain[i_0];
      }

      rtb_x[i] = x[i] + Yh;
    }

    /*  Normalization DCM(1:3) */
    /* '<S7>:1:38' */
    Yh = 0.0;
    scale = 0.0;
    firstNonZero = TRUE;
    if (rtb_x[0] != 0.0) {
      scale = fabs(rtb_x[0]);
      Yh = 1.0;
      firstNonZero = FALSE;
    }

    if (rtb_x[1] != 0.0) {
      cos_psi = fabs(rtb_x[1]);
      if (firstNonZero) {
        scale = cos_psi;
        Yh = 1.0;
        firstNonZero = FALSE;
      } else if (scale < cos_psi) {
        Xh = scale / cos_psi;
        Yh = Yh * Xh * Xh + 1.0;
        scale = cos_psi;
      } else {
        Xh = cos_psi / scale;
        Yh += Xh * Xh;
      }
    }

    if (rtb_x[2] != 0.0) {
      cos_psi = fabs(rtb_x[2]);
      if (firstNonZero) {
        scale = cos_psi;
        Yh = 1.0;
      } else if (scale < cos_psi) {
        Xh = scale / cos_psi;
        Yh = Yh * Xh * Xh + 1.0;
        scale = cos_psi;
      } else {
        Xh = cos_psi / scale;
        Yh += Xh * Xh;
      }
    }

    Yh = scale * sqrt(Yh);
    rtb_x[0] = rtb_x[0] / Yh;
    rtb_x[1] = rtb_x[1] / Yh;
    rtb_x[2] = rtb_x[2] / Yh;

    /*  Update the error covariance */
    /* '<S7>:1:40' */
    for (i = 0; i < 36; i++) {
      c_I[i] = 0;
    }

    for (i = 0; i < 6; i++) {
      c_I[i + 6 * i] = 1;
    }

    for (i = 0; i < 6; i++) {
      for (i_0 = 0; i_0 < 6; i_0++) {
        Yh = 0.0;
        for (i_1 = 0; i_1 < 6; i_1++) {
          Yh += Phik[6 * i_1 + i] * (real_T)tmp_4[6 * i_0 + i_1];
        }

        Phik_0[i + 6 * i_0] = (real_T)c_I[6 * i_0 + i] - Yh;
      }
    }

    for (i = 0; i < 6; i++) {
      for (i_0 = 0; i_0 < 6; i_0++) {
        rtb_P[i + 6 * i_0] = 0.0;
        for (i_1 = 0; i_1 < 6; i_1++) {
          rtb_P[i + 6 * i_0] = Phik_0[6 * i_1 + i] * P[6 * i_0 + i_1] + rtb_P[6 *
            i_0 + i];
        }
      }
    }

    /* Embedded MATLAB: '<S1>/Compute psi' */
    /* Embedded MATLAB Function 'DCM Filter/Compute psi': '<S2>:1' */
    /*  x1: [C31,C32,C33,gx,gy,gz] */
    /*  x2: [C21 C22 C23 gx gy gz] */
    /* '<S2>:1:7' */
    /* '<S2>:1:8' */
    /* '<S2>:1:9' */
    /* '<S2>:1:10' */
    /* '<S2>:1:11' */
    /* '<S2>:1:12' */
    /* '<S2>:1:14' */
    /* '<S2>:1:15' */
    cos_psi = rtb_x[2] * rtb_x_e[0] - rtb_x[0] * rtb_x_e[2];

    /* '<S2>:1:16' */
    Xh = rtb_x[0] * rtb_x_e[1] - rtb_x[1] * rtb_x_e[0];

    /* '<S2>:1:17' */
    /* '<S2>:1:19' */
    /* '<S2>:1:20' */
    cos_theta = cos(-asin(rtb_x_e[0]));
    if (rtb_x_e[0] == 1.0) {
      /* '<S2>:1:22' */
      /* theta = pi/2; */
      /* '<S2>:1:24' */
      /* '<S2>:1:25' */
      cos_psi = rt_atan2d_snf(cos_psi, Xh);
    } else if (rtb_x_e[0] == -1.0) {
      /* '<S2>:1:26' */
      /* theta = -pi/2; */
      /* '<S2>:1:28' */
      /*  anything */
      /* '<S2>:1:29' */
      cos_psi = rt_atan2d_snf(-cos_psi, -Xh);
    } else {
      /* phi = atan2(C32/cos_theta , C33/cos_theta); */
      /* '<S2>:1:32' */
      cos_psi = rt_atan2d_snf(rtb_x[0] / cos_theta, (rtb_x[1] * rtb_x_e[2] -
        rtb_x[2] * rtb_x_e[1]) / cos_theta);
    }

    /* Gain: '<Root>/Gain2' */
    Out1[0] = TungSim_P.Gain2_Gain * rt_atan2d_snf(rtb_x_e[1], rtb_x_e[2]);
    Out1[1] = TungSim_P.Gain2_Gain * theta;
    Out1[2] = TungSim_P.Gain2_Gain * cos_psi;

    /* DiscretePulseGenerator: '<Root>/Pulse Generator' */
    Out2 = ((real_T)TungSim_DWork.clockTickCounter <
            TungSim_P.PulseGenerator_Duty) && (TungSim_DWork.clockTickCounter >=
      0) ? TungSim_P.PulseGenerator_Amp : 0.0;
    if ((real_T)TungSim_DWork.clockTickCounter >=
        TungSim_P.PulseGenerator_Period - 1.0) {
      TungSim_DWork.clockTickCounter = 0;
    } else {
      TungSim_DWork.clockTickCounter = TungSim_DWork.clockTickCounter + 1;
    }

    /* user code (Output function Trailer) */

    /* System '<Root>' */
//    for (i=0; i<3; i++)
//      euler[i] = Out1[i];
    if (Out2 != 0.0)
      GPIO_SetBits(GPIOD, GPIO_Pin_13);
    else
      GPIO_ResetBits(GPIOD, GPIO_Pin_13);
  }

  /* Update for UnitDelay: '<S1>/Unit Delay' */
  for (i = 0; i < 6; i++) {
    TungSim_DWork.UnitDelay_DSTATE[i] = rtb_x_e[i];
  }

  /* Update for UnitDelay: '<S1>/Unit Delay1' */
  memcpy((void *)(&TungSim_DWork.UnitDelay1_DSTATE[0]), (void *)&rtb_P_j[0], 36U
         * sizeof(real_T));

  /* Update for UnitDelay: '<S1>/Unit Delay2' */
  for (i = 0; i < 6; i++) {
    TungSim_DWork.UnitDelay2_DSTATE[i] = rtb_x[i];
  }

  /* Update for UnitDelay: '<S1>/Unit Delay3' */
  memcpy((void *)(&TungSim_DWork.UnitDelay3_DSTATE[0]), (void *)&rtb_P[0], 36U *
         sizeof(real_T));
}

/* Model initialize function */
void TungSim_initialize(void)
{
  /* Registration code */

  /* initialize non-finites */
  rt_InitInfAndNaN(sizeof(real_T));

  /* initialize error status */
  rtmSetErrorStatus(TungSim_M, (NULL));

  /* block I/O */

  /* exported global signals */
  Out1[0] = 0.0;
  Out1[1] = 0.0;
  Out1[2] = 0.0;
  Out2 = 0.0;

  /* states (dwork) */
  (void) memset((void *)&TungSim_DWork, 0,
                sizeof(D_Work_TungSim));

  /* external inputs */
  (void) memset(fgyro, 0,
                3U*sizeof(real_T));
  (void) memset(facc, 0,
                3U*sizeof(real_T));
  (void) memset(fmag, 0,
                3U*sizeof(real_T));

  /* Start for DiscretePulseGenerator: '<Root>/Pulse Generator' */
  TungSim_DWork.clockTickCounter = 0;

  {
    int32_T i;

    /* InitializeConditions for UnitDelay: '<S1>/Unit Delay' */
    for (i = 0; i < 6; i++) {
      TungSim_DWork.UnitDelay_DSTATE[i] = TungSim_P.UnitDelay_X0[i];
    }

    /* InitializeConditions for UnitDelay: '<S1>/Unit Delay1' */
    memcpy((void *)(&TungSim_DWork.UnitDelay1_DSTATE[0]), (void *)
           (&TungSim_P.UnitDelay1_X0[0]), 36U * sizeof(real_T));

    /* InitializeConditions for UnitDelay: '<S1>/Unit Delay2' */
    for (i = 0; i < 6; i++) {
      TungSim_DWork.UnitDelay2_DSTATE[i] = TungSim_P.UnitDelay2_X0[i];
    }

    /* InitializeConditions for UnitDelay: '<S1>/Unit Delay3' */
    memcpy((void *)(&TungSim_DWork.UnitDelay3_DSTATE[0]), (void *)
           (&TungSim_P.UnitDelay3_X0[0]), 36U * sizeof(real_T));
  }
}

/* Model terminate function */
void TungSim_terminate(void)
{
  /* (no terminate code required) */
}

/*
 * File trailer for Real-Time Workshop generated code.
 *
 * [EOF]
 */
